#include "WPILib.h"
#include <Encoder.h>
#include <PIDController.h>

// output ports
#define LEFTFRONT 9
#define LEFTBACK 9
#define RIGHTFRONT 10
#define RIGHTBACK 10
#define SDLEFT 2
#define SDRIGHT 1
#define SMANIP1 3
#define SMANIP2 4

//input ports
#define LFENC_A 0
#define LFENC_B 0
#define RFENC_A 11
#define RFENC_B 12
#define LBENC_A 13
#define LBENC_B 14
#define RBENC_A 0
#define RBENC_B 0

//PID constants
#define KP 0.50
#define KI 0.10
#define KD 0.00

//Other robot constants
#define WHEEL_RADIUS 1.0
#define PI 3.14159
//#define DISTANCE_PER_PULSE_4X WHEEL_RADIUS * PI / 2
#define DISTANCE_PER_PULSE_4X 1
#define TOLERANCE 1.0e-2
#define MAX_COUNTS_PER_SEC 5500.00
#define COUNTER_TOLERANCE MAX_COUNTS_PER_SEC * TOLERANCE


class robot : public SimpleRobot
{
	Timer *timer;
	
	PIDController *leftFrontPID;
	PIDController *leftBackPID;
	PIDController *rightFrontPID;
	PIDController *rightBackPID;
	Encoder *leftFrontEncoder;
	Encoder *leftBackEncoder;
	Encoder *rightFrontEncoder;
	Encoder *rightBackEncoder;
	Jaguar *leftFrontMotor;
	Jaguar *leftBackMotor;
	Jaguar *rightFrontMotor;
	Jaguar *rightBackMotor;
	Joystick *stickDriveLeft;
	Joystick *stickDriveRight;
	Joystick *stickManip1;
	Joystick *stickManip2;
	
	DriverStationLCD *screen;

public:
	robot();
	void InitJoysticks();
	void InitJaguars();
	void InitPIDEncoders();
	void SetSetpoints(double);
	void SetLeftRightSideSpeeds(float, float);
	void Drive(float, float, float);
	void Autonomous();
	void OperatorControl();
	void Move(double, double, double, double);
	void MoveWithCalibratedSpeed(double, double, double, double, double);
};
